#!/usr/bin/env pybricks-micropython

# 该文件用于测试相关系统件能力

from pybricks.hubs import EV3Brick
from pybricks.ev3devices import (
    Motor,
    TouchSensor,
    ColorSensor,
)
from pybricks.parameters import Port, Stop, Direction, Button, Color
from pybricks.tools import wait, StopWatch, DataLog
from pybricks.robotics import DriveBase
from pybricks.media.ev3dev import SoundFile, ImageFile


def measure(motor, name="Motor"):
    print("^ Measuring Motor %s ..." % (name))
    print("^ Motor angle: ", motor.angle())
    print("^ Motor speed: ", motor.speed())


MOTOR_A_SCALE = 4

ev3 = EV3Brick()
motor_a = Motor(Port.A)
motor_b = Motor(Port.B)
color_sensor = ColorSensor(Port.S1)
touch_sensor = TouchSensor(Port.S2)

# ev3.speaker.beep(frequency=500, duration=200)
# ev3.speaker.beep(frequency=1000, duration=200)
# ev3.speaker.beep(frequency=3000, duration=200)

ev3.speaker.play_file("../media/assets/dji_startup.adapted.wav")

measure(motor_a, "A")
measure(motor_b, "B")

# waitor = StopWatch()
# while waitor.time() < 3000:
#     print(waitor.time())
#     wait(100)

# while True:
#     print(touch_sensor.pressed())
#     wait(1000)

# exit()

# motor.run 传入的是速度 deg/s
# motor_b.run(-POWER_HIGH) # 负值：底部顺时针，鹦鹉嘴抬头；正值：底部逆时针，鹦鹉嘴低头
# wait(100)
# motor_b.stop(Stop.BRAKE)

# run(speed)
# Runs the motor at a constant speed.
# The motor accelerates to the given speed and keeps running at this speed until you give a new command.
# Parameters:	speed (rotational speed: deg/s) – Speed of the motor.

# run_time(speed, time, then=Stop.HOLD, wait=True)
# Runs the motor at a constant speed for a given amount of time.
# The motor accelerates to the given speed, keeps running at this speed, and then decelerates. The total maneuver lasts for exactly the given amount of time.
# Parameters:
# speed (rotational speed: deg/s) – Speed of the motor.
# time (time: ms) – Duration of the maneuver.
# then (Stop) – What to do after coming to a standstill.
# wait (bool) – Wait for the maneuver to complete before continuing with the rest of the program.

# run_angle(speed, rotation_angle, then=Stop.HOLD, wait=True)
# Runs the motor at a constant speed by a given angle.
# Parameters:
# speed (rotational speed: deg/s) – Speed of the motor.
# rotation_angle (angle: deg) – Angle by which the motor should rotate.
# then (Stop) – What to do after coming to a standstill.
# wait (bool) – Wait for the maneuver to complete before continuing with the rest of the program.

# run_target(speed, target_angle, then=Stop.HOLD, wait=True)
# Runs the motor at a constant speed towards a given target angle.
# The direction of rotation is automatically selected based on the target angle. It does matter if speed is positive or negative.
# Parameters:
# speed (rotational speed: deg/s) – Speed of the motor.
# target_angle (angle: deg) – Angle that the motor should rotate to.
# then (Stop) – What to do after coming to a standstill.
# wait (bool) – Wait for the motor to reach the target before continuing with the rest of the program.

# run_until_stalled(speed, then=Stop.COAST, duty_limit=None)

# motor_a.run_time(-140, 1000)
# motor_b.run_time(50, 1000)
# motor_a.run_angle(360, (180 + 45) * MOTOR_A_SCALE, then=Stop.BRAKE, wait=True)
# motor_b.run_angle(360, (90))

# motor_a.run(-POWER_HIGH)
# wait(3000)
# motor_a.stop(Stop.BRAKE)
